#include "FastSerial.h"
#include "Coms.h"
#include <Arduino.h>
#include "eepromManager.h"
#include "PID.h"
#include "myGps.h"

#define COMS Serial3

typedef struct {
  double roll,heading,pitch,vrate,alt,speed,slip;
} SsetpointPacket;

Coms::Coms()
{
  oldAPen = false;
}

void Coms::init(Aircraft *aircraft)
{
  COMS.begin(115200);
  COMS.println("Start");
  packet = false;
  this->aircraft = aircraft;
  
  //Default registered packets
  registered.push_back(7);//Telemetry
  registered.push_back(15);//GPS
  lastTx = 0;
  dataAt = 0;
}

void Coms::update()
{
  unsigned long time = millis();
  if (time-lastTx >= 100)
  {
    if (registered.size()!=2){Serial.print("OH NO:");Serial.println(registered.size());}
    lastTx = time;
    out_telem();
    out_gps();
    out_sensors();
    /*for (int i=0; i<registered.size(); i++)
    {
      switch (registered[i])
      {
        case  2:out_planeMode();break;
        case  3:out_sensors();break;
        case  7:out_telem();break;
        case 12:out_servoCalib();break;
        case 13:out_pid();break;
        case 14:out_servo();break;
        case 15:out_gps();break;
        case 16:out_setpoints();break;
        default:break;
      }
    }*/
  }
  
  bool isen = aircraft->ap.isEn();
  if (isen != oldAPen)
  {
    oldAPen = isen;
    out_systemStatus();
  }
  
  int count = 0;
  if ((!packet && COMS.available() >= 5) || (packet && COMS.available() >= packetSize))
  {
    count++;
    if (count>50){return;}
    //If we do not have a packet to read, see if we can read one
    //5 Bytes is the absolute minimum length in bytes of the head
    //Some packets have a body size of 0 (ie ID 0, emergency stop)
    if (!packet && COMS.available() >= 5)
    {
      if (COMS.read()==int('@') && COMS.read()==int('>')) // Check first two bytes of header
      {
        packetXOR = COMS.read(); // Byte 3 for Header
        packetID = COMS.read(); // Byte 4 for Header
        packetSize = COMS.read(); // Byte 5 for Header
        packet = true; // Got a packet!
        dataAt = 0;
      }
    }
    int haveBytes = COMS.available();
    if (packet) // Make sure we recieved the whole packet, I do not want blocking commands
    {
      if (haveBytes+dataAt >= packetSize)
      {
        for (;dataAt<packetSize;dataAt++){data[dataAt]=COMS.read();}
        dataAt = 0;
        packet = false; // We are now flagging the packet as being handled
        uint8_t parity = (packetID ^ packetSize);
        for (int i=0;i<packetSize;i++){parity ^= data[i];}
        if (parity != packetXOR)
        {
          //Serial.print("BAD PACKET ");
          //Serial.print(packetID);
          //Serial.print("  GOT: ");
          //Serial.print(packetXOR,DEC);
          //Serial.print(" vs ");
          //Serial.println(parity,DEC);
          //for (int i=0;i<packetSize;i++)
          //{
          //  if (data[i]<0x10){Serial.print("0");}
          //  Serial.print(data[i],HEX);
          //  Serial.print(" ");
          //}
          //Serial.println();
          return;
        }//do not read occrupted packet
        switch (packetID)
        {
          //GENERAL
          case  0: in_eStop();break;
          case  1: in_changeMode();break;
          case  2: in_req_planeMode();break;
          case  3: in_sensors();break;
          case  7: in_telem();break;
          case  8: in_systemStatus();break;
          case 10: in_servoConfig();break;
          case 11: in_pidConfig();break;
          case 12: in_servoRead();break;
          case 13: in_pidRead();break;
          case 14: in_servo();break;
          case 15: in_gps();break;
          case 16: in_setpointRead();break;
          case 17: in_setpointChange();break;
          
          //SENSORS
          default: in_unknown();break; // Do I care?
        }
      }
      else
      {
        //give me all ur bytes
        for (int i=0;i<haveBytes;i++){data[dataAt++]=COMS.read();}
      }
    }
  }
}

void Coms::completePacket(uint8_t *p,int id,int size)
{
  int realSize = size+5;
  p[0] = '$';
  p[1] = '<';
  p[2] = 0;
  p[3] = id;
  p[4] = size;
  for (int i=3;i<realSize;i++){p[2]^=p[i];}
  COMS.write(p,realSize);
}

void Coms::writeInt(uint8_t *p,int num)
{
  p[0]=num>>8;
  p[1]=num&0xFF;
}

void Coms::writeDouble(uint8_t *p,double num)
{
  memcpy(p,&num,4);
}

void Coms::in_eStop()
{
  if (packetSize!=0){out_err_size();}
  //Stop interrups and do nothing
  sei();
  while(1);
}
void Coms::in_changeMode()
{
  if (packetSize==0||packetSize>2){out_err_size();return;}//corrupted
  if (packetSize==2)
  {
    if (!aircraft->ap.switchIndvMode(data[1]))
    {
      out_err_byte(2);
      return;
    }
  }
  if (!aircraft->ap.switchToMode(data[0])){out_err_byte(1);return;}
  out_ack();
}
void Coms::in_sensors()
{
  if (packetSize!=0){out_err_size();}
  out_sensors();
}
void Coms::in_req_planeMode()
{
  if (packetSize!=0){out_err_size();}
  out_planeMode();
}
void Coms::in_telem()
{
  if (packetSize!=0){out_err_size();}
  out_telem();
}
void Coms::in_systemStatus()
{
  if (packetSize!=0){out_err_size();}
  out_systemStatus();
}
void Coms::in_servoConfig()
{
  const int isz = sizeof(SservoCalib);
  if (packetSize<=1 || ((packetSize-1)%isz)!=0){out_err_size();return;}
  if ((data[0]&0xF0)!=0 || data[0]==0){out_err_byte(1);return;}
  SservoCalib t;
  int sz = 1;
  if (data[0]&1)
  {
    memcpy(&t,&data[sz],isz);
    aircraft->setSvoRange(1,t.min,t.max);
    aircraft->svoInvert(1,t.inverted);
    sz+=isz;
  }
  if (data[0]&2)
  {
    memcpy(&t,&data[sz],isz);
    aircraft->setSvoRange(2,t.min,t.max);
    aircraft->svoInvert(2,t.inverted);
    sz+=isz;
  }
  if (data[0]&4)
  {
    memcpy(&t,&data[sz],isz);
    aircraft->setSvoRange(3,t.min,t.max);
    aircraft->svoInvert(3,t.inverted);
    sz+=isz;
  }
  if (data[0]&8)
  {
    memcpy(&t,&data[sz],isz);
    aircraft->setSvoRange(4,t.min,t.max);
    aircraft->svoInvert(4,t.inverted);
    sz+=isz;
  }
  aircraft->saveSvoPresets();
  out_ack();
}
void Coms::in_pidConfig()
{
  //Serial.println("PID CONFIG NOW!");
  //delay(100);
  if (packetSize<22||packetSize>128||(packetSize-1)%21!=0){out_err_size();return;}
  int cnt = 0;
  if (data[0]& 1){cnt++;}
  if (data[0]& 2){cnt++;}
  if (data[0]& 4){cnt++;}
  if (data[0]& 8){cnt++;}
  if (data[0]&16){cnt++;}
  if (data[0]&32){cnt++;}
  if (data[0]&64){cnt++;}
  if ((packetSize-1)/21!=cnt){out_err_size();return;}
  if ((data[0]&0x80)!=0 || data[0]==0){out_err_byte(2);return;}
  int sz = 1;
  if (data[0]& 1){if (!pidCfg(sz,aircraft->ap.pid.rollHold)){return;}}
  if (data[0]& 2){if (!pidCfg(sz,aircraft->ap.pid.headingHold)){return;}}
  if (data[0]& 4){if (!pidCfg(sz,aircraft->ap.pid.pitchHold)){return;}}
  if (data[0]& 8){if (!pidCfg(sz,aircraft->ap.pid.vrateHold)){return;}}
  if (data[0]&16){if (!pidCfg(sz,aircraft->ap.pid.altHold)){return;}}
  if (data[0]&32){if (!pidCfg(sz,aircraft->ap.pid.throttleHold)){return;}}
  if (data[0]&64){if (!pidCfg(sz,aircraft->ap.pid.slipHold)){return;}}
  aircraft->ap.savePIDtoEEPROM();
  out_ack();
}
bool Coms::pidCfg(int &offset,PID &pid)
{
  if ((data[offset+20]&0xF0)!=0){out_err_byte(offset+20);return false;}
  const int sz = sizeof(PID::Spid);
  PID::Spid t;
  memcpy(&t,&data[offset],sz);
  pid.setFrom(t);
  offset+=sz;
  return true;
}
void Coms::in_servoRead()
{
  if (packetSize!=0){out_err_size();}
  out_servo();
}
void Coms::in_pidRead()
{
  if (packetSize!=0){out_err_size();}
  out_pid();
}
void Coms::in_servo()
{
  if (packetSize!=0){out_err_size();}
  out_servo();
}
void Coms::in_gps()
{
  if (packetSize!=0){out_err_size();}
  out_gps();
}
void Coms::in_setpointRead()
{
  if (packetSize!=0){out_err_size();}
  out_setpoints();
}
void Coms::in_setpointChange()
{
  if (packetSize!=5){out_err_size();}
  double sp = readDouble(1);
  switch (data[0])
  {
    case 1:aircraft->ap.setTargetRoll(sp);break;
    case 2:aircraft->ap.setTargetHeading(sp);break;
    case 3:aircraft->ap.setTargetPitch(sp);break;
    case 4:aircraft->ap.setTargetVRate(sp);break;
    case 5:aircraft->ap.setTargetAlt(sp);break;
    case 6:aircraft->ap.setTargetThrottle(sp);break;
    case 7:aircraft->ap.setTargetSlip(sp);break;
    default:out_err_byte(1);return;
  }
  out_ack();
}
void Coms::in_unknown(){} // Do I care?

double Coms::readDouble(int pos)
{
  double num;
  memcpy(&num,&data[pos],sizeof(double));
  return num;
}

//////////////////////////////
//--------------------------//
//-----O--U--T--P--U--T-----//
//--------------------------//
//////////////////////////////

void Coms::out_ack()//0
{
  uint8_t p[6];
  p[5]=packetID;
  completePacket(p,0,1);
}

void Coms::out_err(int prob,int index)//1
{
  uint8_t p[8];
  p[5]=packetID;
  p[6]=prob;
  p[7]=index;
  completePacket(p,1,3);
}

void Coms::out_err(int prob)//1
{
  uint8_t p[7];
  p[5]=packetID;
  p[6]=prob;
  completePacket(p,1,2);
}

void Coms::out_err_byte(int byteNumber)
{
  Serial.print("Byte error with packet ");
  Serial.print(packetID);
  Serial.print(", ");
  Serial.print(byteNumber);
  out_err(2,byteNumber);
}//1
void Coms::out_err_size()
{
  Serial.print("Size error with packet ");
  Serial.println(packetID);
  out_err(1);
}//1

void Coms::out_planeMode()//2 ==R==
{
  uint8_t p[7];
  p[5]=aircraft->ap.getMode();
  p[6]=aircraft->ap.getIndvMode();
  completePacket(p,2,2);
}

void Coms::out_sensors()//3 ==R==
{
  uint8_t p[45];
  writeDouble(&p[ 5],aircraft->sensors.gx());
  writeDouble(&p[ 9],aircraft->sensors.gy());
  writeDouble(&p[13],aircraft->sensors.gz());
  writeDouble(&p[17],aircraft->sensors.ay());
  writeDouble(&p[21],aircraft->sensors.ax());
  writeDouble(&p[25],aircraft->sensors.az());
  writeDouble(&p[29],0);
  writeDouble(&p[33],0);
  writeDouble(&p[37],0);
  writeDouble(&p[41],0);
  completePacket(p,3,40);
}

void Coms::out_telem()//7 ==R==
{
  uint8_t p[33];
  Stelem t;
  t.lat = aircraft->sensors.getLat();
  t.lon = aircraft->sensors.getLon();
  t.alt = aircraft->sensors.getAlt();
  t.roll = aircraft->sensors.getRoll();
  t.pitch = aircraft->sensors.getPitch();
  t.heading = aircraft->sensors.getCourse();
  if (t.heading<0){t.heading+=360.0;}
  t.speed = aircraft->sensors.getSpeed();
  memcpy(&p[5],&t,28);
  completePacket(p,7,28);
}

void Coms::out_systemStatus()//8 ==R==
{
  uint8_t p[14];
  p[5] = oldAPen;
  writeInt(&p[6],int(aircraft->sensors.getBattery()*100));
  writeInt(&p[8],0);
  writeInt(&p[10],0);
  writeInt(&p[12],0);
  completePacket(p,8,9);
}

void Coms::out_servoCalib()//12 ==R==
{
  const int sz = sizeof(SservoAllCalib);
  uint8_t p[5+sz];
  SservoAllCalib sc = EEPROMmanager::readServoCalibrations();
  memcpy(&p[5],&sc,sz);
  completePacket(p,12,sz);
}

void Coms::out_servo()//14 ==R==
{
  uint8_t p[13];
  writeInt(&p[ 5],aircraft->getRCout(1));
  writeInt(&p[ 7],aircraft->getRCout(2));
  writeInt(&p[ 9],aircraft->getRCout(3));
  writeInt(&p[11],aircraft->getRCout(4));
  completePacket(p,14,8);
}

void Coms::out_pid()//13 x1 ==R==
{
  const int sz = sizeof(SpidConstants);
  uint8_t p[5+sz];
  SpidConstants pc = EEPROMmanager::readPIDconstants();
  memcpy(&p[5],&pc,sz);
  completePacket(p,13,sz);
}
void Coms::out_gps()//15 ==R==
{
  MyGPS &gps = *(aircraft->sensors.gps);
  int sz = 43+(4*gps.my_vsat.size());
  uint8_t p[5+sz];
  p[5]=gps.status();
  writeDouble(&p[6],gps.my_lat);
  writeDouble(&p[10],gps.my_lon);
  writeDouble(&p[14],gps.my_alt);
  writeDouble(&p[18],gps.my_speed);
  writeDouble(&p[22],gps.my_hdop);
  writeDouble(&p[26],gps.my_vdop);
  writeDouble(&p[30],gps.my_pdop);
  writeDouble(&p[34],gps.my_course);
  writeDouble(&p[38],gps.my_deviation);
  p[42] = gps.my_vsat.size();
  if (p[42]>32){p[42]=32;}
  for (int i=0;i<p[42];i++)
  {
    MyGPS::Sat s = gps.my_vsat.at(i);
    memcpy(&p[43+(i*4)],&s,4);
  }
  completePacket(p,15,sz);
}

void Coms::out_setpoints()//16 ==R==
{
  uint8_t p[33];
  SsetpointPacket s;
  s.roll = aircraft->ap.getTargetRoll();
  s.heading = aircraft->ap.getTargetHeading();
  s.pitch = aircraft->ap.getTargetPitch();
  s.vrate = aircraft->ap.getTargetVRate();
  s.alt = aircraft->ap.getTargetAlt();
  s.speed = aircraft->ap.getTargetThrottle();
  s.slip = aircraft->ap.getTargetSlip();
  memcpy(&p[5],&s,sizeof(SsetpointPacket));
  completePacket(p,16,28);
}
